#ifndef MATRIX_H
#define MATRIX_H

//#include"geometric.h"
#include"main.h"

template <typename T, int M,int N>
class CMatrix //适合单片机处理的矩阵
{ //全部使用定长结构
public:  
	T pd[M][N]; //M行N列
	CMatrix(){}
	void print(void)
	{
		for(int i=0;i<M;i++)
		{
			for(int j=0;j<N;j++)
			{
				printf("%.3f ",pd[i][j]);
			}
			printf("\n");
		}
	}
	CMatrix<T,M,N> add(CMatrix<T,M,N> &m)
	{
		int i,j;
		for(i=0;i<M;i++)
		{
			for(j=0;j<N;j++)
			{
				pd[i][j]+=m.pd[i][j];
			}
		}
	}
	CMatrix<T,M,N> minus(CMatrix<T,M,N> &m)
	{
		int i,j;
		for(i=0;i<M;i++)
		{
			for(j=0;j<N;j++)
			{
				pd[i][j]-=m.pd[i][j];
			}
		}
	}
	template <int K>
	void mul(CMatrix<T,N,K> &m,CMatrix<T,M,K> &out) //为了减少栈的使用，需要从外部输入
	{
		int i,j,k;
		for(i=0;i<M;i++) //行
		{
			for(j=0;j<K;j++) //列
			{
				out.pd[i][j]=0;
				for(k=0;k<N;k++)
				{
					out.pd[i][j]+=pd[i][k]*m.pd[k][j];
				}
			}
		}
	}
	T sq_sum(void) //平方和
	{
		T sum=0;
		int i,j;
		for(i=0;i<M;i++)
		{
			for(j=0;j<N;j++)
			{
				sum+=pd[i][j]*pd[i][j];
			}
		}
		return sum;
	}
	void t(CMatrix<T,N,M> &m) //转置,为了减少栈的使用，需要从外部输入
	{
		int i,j;
		for(i=0;i<M;i++) //行
		{
			for(j=0;j<N;j++) //列
			{
				m.pd[j][i]=pd[i][j];
			}
		}
	}
	void inv(CMatrix<T,M,M> &m) //逆,为了减少栈的使用，需要从外部输入,且将自身变为单位阵
	{
		if(M!=N) return;
		int i,j,k;
	//首先将输入矩阵赋值为单位阵
		memset(m.pd,0,sizeof(T)*M*M);
		for(i=0;i<M;i++) //行
		{
			m.pd[i][i]=1;
		}
	//将下三角清零
		for(i=0;i<M-1;i++) //除了最后一行
		{
			//首先判断第一个元素是否为0，若为0需要找一个非零行换过来
			if(fabs(pd[i][i])<Eps)
			{
				for(j=i;j<M;j++)
				{
					if(fabs(pd[j][i])>Eps)
					{
						for(k=i;k<M;k++)
						{
							T t=pd[i][k];
							pd[i][k]=pd[j][k];
							pd[j][k]=t;
							//把另一部分也换了
							t=m.pd[i][k];
							m.pd[i][k]=m.pd[j][k];
							m.pd[j][k]=t;
						}
						break;
					}
				}
			}
			//把i行数据化成1打头
			T f=pd[i][i];
			for(k=i;k<M;k++) //本身
			{
				pd[i][k]/=f;
			}
			for(k=0;k<=i;k++) //另一部分
			{
				m.pd[i][k]/=f;
			}
			for(j=i+1;j<M;j++) //从下一行开始,消元
			{//第i行消第j行
				f=pd[j][i]; //比例
				//i行乘以比例减到j行
				for(k=i;k<M;k++) //本身
				{
					pd[j][k]-=f*pd[i][k];
				}
				for(k=0;k<j;k++) //另一部分
				{
					m.pd[j][k]-=f*m.pd[i][k];
				}
			}
		}
	//把最后1行数据化成1打头
		T f=pd[i][i];
		pd[i][i]=1;
		for(k=0;k<=i;k++)
		{
			m.pd[i][k]/=f;
		}
	//将上三角清零
		for(i=M-1;i>0;i--) //除了第一行
		{ //对角线上元素已经都是1了
			for(j=i-1;j>=0;j--) //从上一行开始,消元
			{//第i行消第j行
				f=pd[j][i]; //比例
				//i行乘以比例减到j行
				pd[j][i]=0;
				for(k=0;k<i;k++) //本身
				{
					pd[j][k]-=f*pd[i][k];
				}
				for(k=0;k<M;k++) //另一部分
				{
					m.pd[j][k]-=f*m.pd[i][k];
				}
			}
		}
	}
};

#endif

